package org.cove.ape {
	import flash.errors.IllegalOperationError;		

	/**
	 * @author cbra
	 */
	public class Matrix3f {   	
		public var m00:Number;
		public var m01:Number;
		public var m02:Number;
		public var m10:Number;
		public var m11:Number;
		public var m12:Number;   	
		public var m20:Number;
		public var m21:Number;
		public var m22:Number;

		/**
		 * Constructor instantiates a new <code>Matrix3f</code> object. The
		 * initial values for the matrix is that of the identity matrix.
		 *  
		 */
		public function Matrix3f() {
//        	loadIdentity();
		}

		/**
		 * <code>copy</code> transfers the contents of a given matrix to this
		 * matrix. If a null matrix is supplied, this matrix is set to the identity
		 * matrix.
		 * 
		 * @param matrix
		 *            the matrix to copy.
		 */
		public function copy(matrix:Matrix3f ):void {
			if (null == matrix) {
//            loadIdentity();
        } else {
				m00 = matrix.m00;
				m01 = matrix.m01;
				m02 = matrix.m02;
				m10 = matrix.m10;
				m11 = matrix.m11;
				m12 = matrix.m12;
				m20 = matrix.m20;
				m21 = matrix.m21;
				m22 = matrix.m22;
			}
		}

		/**
		 * <code>loadIdentity</code> sets this matrix to the identity matrix.
		 * Where all values are zero except those along the diagonal which are one.
		 *  
		 */
		public function loadIdentity():void {
			m01 = m02 = m10 = m12 = m20 = m21 = 0;
			m00 = m11 = m22 = 1;
		}

		/**
		 * <code>mult</code> multiplies this matrix by a given
		 * <code>Vector3f</code> object. The result vector is returned. If the
		 * given vector is null, null will be returned.
		 * 
		 * @param vec
		 *            the vector to multiply this matrix by.
		 * @return the result vector.
		 */
		public function multVector(vec:Vector3f ):Vector3f {
			return mult(vec, null);
		}

		/**
		 * Multiplies this 3x3 matrix by the 1x3 Vector vec and stores the result in
		 * product.
		 * 
		 * @param vec
		 *            The Vector3f to multiply.
		 * @param product
		 *            The Vector3f to store the result, it is safe for this to be
		 *            the same as vec.
		 * @return The given product vector.
		 */
		public function mult(vec:Vector3f , product:Vector3f ):Vector3f {
        
			if (null == product) {
				product = new Vector3f();
			}

			var x:Number = vec.x;
			var y:Number = vec.y;
			var z:Number = vec.z;

			product.x = m00 * x + m01 * y + m02 * z;
			product.y = m10 * x + m11 * y + m12 * z;
			product.z = m20 * x + m21 * y + m22 * z;
			return product;
		}

		
		/**
		 * A function for creating a rotation matrix that rotates a vector called
		 * "start" into another vector called "end".
		 * 
		 * @param start
		 *            normalized non-zero starting vector
		 * @param end
		 *            normalized non-zero ending vector
		 * @see "Tomas M�ller, John Hughes \"Efficiently Building a Matrix to Rotate \
		 *      One Vector to Another\" Journal of Graphics Tools, 4(4):1-4, 1999"
		 */
		public function fromStartEndVectors(start:Vector3f , end:Vector3f ):Matrix3f {
			var v:Vector3f = new Vector3f();
			var e:Number;
			var h:Number;
			var f:Number;

			start.crossVectorBuffered(end, v);
			e = start.dot(end);
			f = (e < 0) ? -e : e;

			// if "from" and "to" vectors are nearly parallel
			if (f > 1.0 - APEngine.ZERO_TOLERANCE) {
				var u:Vector3f = new Vector3f();
				var x:Vector3f = new Vector3f();
				var c1:Number;
				var c2:Number;
				var c3:Number;
				var i:int;
				var j:int;

				x.x = (start.x > 0.0) ? start.x : -start.x;
				x.y = (start.y > 0.0) ? start.y : -start.y;
				x.z = (start.z > 0.0) ? start.z : -start.z;

				if (x.x < x.y) {
					if (x.x < x.z) {
						x.x = 1.0;
						x.y = x.z = 0.0;
					} else {
						x.z = 1.0;
						x.x = x.y = 0.0;
					}
				} else {
					if (x.y < x.z) {
						x.y = 1.0;
						x.x = x.z = 0.0;
					} else {
						x.z = 1.0;
						x.x = x.y = 0.0;
					}
				}

				u.x = x.x - start.x;
				u.y = x.y - start.y;
				u.z = x.z - start.z;
				v.x = x.x - end.x;
				v.y = x.y - end.y;
				v.z = x.z - end.z;

				c1 = 2.0 / u.dot(u);
				c2 = 2.0 / v.dot(v);
				c3 = c1 * c2 * u.dot(v);

				for (i = 0;i < 3; i++) {
					var val:Number;
					for (j = 0;j < 3; j++) {
						val = -c1 * u.getByIndex(i) * u.getByIndex(j) - c2 * v.getByIndex(i) * v.getByIndex(j) + c3 * v.getByIndex(i) * u.getByIndex(j);
						setByIndex(i, j, val);
					}
					val = getByIndex(i, i);
					setByIndex(i, i, val + 1.0);
				}
			} else {
				// the most common case, unless "start"="end", or "start"=-"end"

				var hvx:Number;
				var hvz:Number; 
				var hvxy:Number;
				var hvxz:Number;
				var hvyz:Number;
				
				h = 1.0 / (1.0 + e);
				hvx = h * v.x;
				hvz = h * v.z;
				hvxy = hvx * v.y;
				hvxz = hvx * v.z;
				hvyz = hvz * v.y;
							
				setByIndex(0, 0, e + hvx * v.x);
				setByIndex(0, 1, hvxy - v.z);
				setByIndex(0, 2, hvxz + v.y);

				setByIndex(1, 0, hvxy + v.z);
				setByIndex(1, 1, e + h * v.y * v.y);
				setByIndex(1, 2, hvyz - v.x);

				setByIndex(2, 0, hvxz - v.y);
				setByIndex(2, 1, hvyz + v.x);
				setByIndex(2, 2, e + hvz * v.z);
			}
			return this;
		}

		/**
		 * <code>set</code> places a given value into the matrix at the given
		 * position. If the position is invalid a <code>RuntimeException</code> is
		 * thrown.
		 * 
		 * @param i
		 *            the row index.
		 * @param j
		 *            the colum index.
		 * @param value
		 *            the value for (i, j).
		 */
		public function setByIndex( i:int,j:int,  value:Number):void {
			switch (i) {
				case 0:
					switch (j) {
						case 0: 
							m00 = value; 
							return;
						case 1: 
							m01 = value; 
							return;
						case 2: 
							m02 = value; 
							return;
					}
				case 1:
					switch (j) {
						case 0: 
							m10 = value; 
							return;
						case 1: 
							m11 = value; 
							return;
						case 2: 
							m12 = value; 
							return;
					}
				case 2:
					switch (j) {
						case 0: 
							m20 = value; 
							return;
						case 1: 
							m21 = value; 
							return;
						case 2: 
							m22 = value; 
							return;
					}
			}
  
			throw new IllegalOperationError("Invalid indices into matrix.");
		}

		/**
		 * <code>get</code> retrieves a value from the matrix at the given
		 * position. If the position is invalid a <code>JmeException</code> is
		 * thrown.
		 * 
		 * @param i
		 *            the row index.
		 * @param j
		 *            the colum index.
		 * @return the value at (i, j).
		 */
		public function getByIndex(i:int, j:int):Number {
			switch (i) {
				case 0:
					switch (j) {
						case 0: 
							return m00;
						case 1: 
							return m01;
						case 2: 
							return m02;
					}
				case 1:
					switch (j) {
						case 0: 
							return m10;
						case 1: 
							return m11;
						case 2: 
							return m12;
					}
				case 2:
					switch (j) {
						case 0: 
							return m20;
						case 1: 
							return m21;
						case 2: 
							return m22;
					}
			}

			throw new IllegalOperationError("Invalid indices into matrix.");
		}

		/**
		 * <code>fromAngleAxis</code> sets this matrix4f to the values specified
		 * by an angle and an axis of rotation.  This method creates an object, so
		 * use fromAngleNormalAxis if your axis is already normalized.
		 * 
		 * @param angle
		 *            the angle to rotate (in radians).
		 * @param axis
		 *            the axis of rotation.
		 */
		public function fromAngleAxis( angle:Number,axis:Vector3f):void {
			var normAxis:Vector3f = axis.normalize();
			fromAngleNormalAxis(angle, normAxis);
		}

		/**
		 * <code>fromAngleNormalAxis</code> sets this matrix4f to the values
		 * specified by an angle and a normalized axis of rotation.
		 * 
		 * @param angle
		 *            the angle to rotate (in radians).
		 * @param axis
		 *            the axis of rotation (already normalized).
		 */
		public function fromAngleNormalAxis( angle:Number,  axis:Vector3f):void {
			var fCos:Number = Math.cos(angle);
			var fSin:Number = Math.sin(angle);
			var fOneMinusCos:Number = (1.0) - fCos;
			var fX2:Number = axis.x * axis.x;
			var fY2:Number = axis.y * axis.y;
			var fZ2:Number = axis.z * axis.z;
			var fXYM:Number = axis.x * axis.y * fOneMinusCos;
			var fXZM:Number = axis.x * axis.z * fOneMinusCos;
			var fYZM:Number = axis.y * axis.z * fOneMinusCos;
			var fXSin:Number = axis.x * fSin;
			var fYSin:Number = axis.y * fSin;
			var fZSin:Number = axis.z * fSin;
        
			m00 = fX2 * fOneMinusCos + fCos;
			m01 = fXYM - fZSin;
			m02 = fXZM + fYSin;
			m10 = fXYM + fZSin;
			m11 = fY2 * fOneMinusCos + fCos;
			m12 = fYZM - fXSin;
			m20 = fXZM - fYSin;
			m21 = fYZM + fXSin;
			m22 = fZ2 * fOneMinusCos + fCos;
		}
	}
}
